home *** CD-ROM | disk | FTP | other *** search
/ io Programmo 60 / IOPROG_60.ISO / soft / c++ / gsl-1.1.1-setup.exe / {app} / src / linalg / qrpt.c < prev    next >
Encoding:
C/C++ Source or Header  |  2002-04-18  |  12.0 KB  |  487 lines
  1. /* linalg/qrpt.c
  2.  * 
  3.  * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, Brian Gough
  4.  * 
  5.  * This program is free software; you can redistribute it and/or modify
  6.  * it under the terms of the GNU General Public License as published by
  7.  * the Free Software Foundation; either version 2 of the License, or (at
  8.  * your option) any later version.
  9.  * 
  10.  * This program is distributed in the hope that it will be useful, but
  11.  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13.  * General Public License for more details.
  14.  * 
  15.  * You should have received a copy of the GNU General Public License
  16.  * along with this program; if not, write to the Free Software
  17.  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18.  */
  19.  
  20. #include <config.h>
  21. #include <stdlib.h>
  22. #include <string.h>
  23. #include <gsl/gsl_math.h>
  24. #include <gsl/gsl_vector.h>
  25. #include <gsl/gsl_matrix.h>
  26. #include <gsl/gsl_permute_vector.h>
  27. #include <gsl/gsl_blas.h>
  28.  
  29. #include <gsl/gsl_linalg.h>
  30.  
  31. #define REAL double
  32.  
  33. #include "givens.c"
  34. #include "apply_givens.c"
  35.  
  36. /* Factorise a general M x N matrix A into
  37.  *
  38.  *   A P = Q R
  39.  *
  40.  * where Q is orthogonal (M x M) and R is upper triangular (M x N).
  41.  * When A is rank deficient, r = rank(A) < n, then the permutation is
  42.  * used to ensure that the lower n - r rows of R are zero and the first
  43.  * r columns of Q form an orthonormal basis for A.
  44.  *
  45.  * Q is stored as a packed set of Householder transformations in the
  46.  * strict lower triangular part of the input matrix.
  47.  *
  48.  * R is stored in the diagonal and upper triangle of the input matrix.
  49.  *
  50.  * P: column j of P is column k of the identity matrix, where k =
  51.  * permutation->data[j]
  52.  *
  53.  * The full matrix for Q can be obtained as the product
  54.  *
  55.  *       Q = Q_k .. Q_2 Q_1
  56.  *
  57.  * where k = MIN(M,N) and
  58.  *
  59.  *       Q_i = (I - tau_i * v_i * v_i')
  60.  *
  61.  * and where v_i is a Householder vector
  62.  *
  63.  *       v_i = [1, m(i+1,i), m(i+2,i), ... , m(M,i)]
  64.  *
  65.  * This storage scheme is the same as in LAPACK.  See LAPACK's
  66.  * dgeqpf.f for details.
  67.  * 
  68.  */
  69.  
  70. int
  71. gsl_linalg_QRPT_decomp (gsl_matrix * A, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
  72. {
  73.   const size_t M = A->size1;
  74.   const size_t N = A->size2;
  75.  
  76.   if (tau->size != GSL_MIN (M, N))
  77.     {
  78.       GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
  79.     }
  80.   else if (p->size != N)
  81.     {
  82.       GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
  83.     }
  84.   else if (norm->size != N)
  85.     {
  86.       GSL_ERROR ("norm size must be N", GSL_EBADLEN);
  87.     }
  88.   else
  89.     {
  90.       size_t i;
  91.  
  92.       *signum = 1;
  93.  
  94.       gsl_permutation_init (p);    /* set to identity */
  95.  
  96.       /* Compute column norms and store in workspace */
  97.  
  98.       for (i = 0; i < N; i++)
  99.     {
  100.       gsl_vector_view c = gsl_matrix_column (A, i);
  101.       double x = gsl_blas_dnrm2 (&c.vector);
  102.       gsl_vector_set (norm, i, x);
  103.     }
  104.  
  105.       for (i = 0; i < GSL_MIN (M, N); i++)
  106.     {
  107.       /* Bring the column of largest norm into the pivot position */
  108.  
  109.       double max_norm = gsl_vector_get(norm, i);
  110.       size_t j, kmax = i;
  111.  
  112.       for (j = i + 1; j < N; j++)
  113.         {
  114.           double x = gsl_vector_get (norm, j);
  115.  
  116.           if (x > max_norm)
  117.         {
  118.           max_norm = x;
  119.           kmax = j;
  120.         }
  121.         }
  122.  
  123.       if (kmax != i)
  124.         {
  125.           gsl_matrix_swap_columns (A, i, kmax);
  126.           gsl_permutation_swap (p, i, kmax);
  127.               gsl_vector_swap_elements(norm,i,kmax);
  128.  
  129.           (*signum) = -(*signum);
  130.         }
  131.  
  132.       /* Compute the Householder transformation to reduce the j-th
  133.          column of the matrix to a multiple of the j-th unit vector */
  134.  
  135.       {
  136.         gsl_vector_view c_full = gsl_matrix_column (A, i);
  137.             gsl_vector_view c = gsl_vector_subvector (&c_full.vector, 
  138.                                                       i, M - i);
  139.         double tau_i = gsl_linalg_householder_transform (&c.vector);
  140.  
  141.         gsl_vector_set (tau, i, tau_i);
  142.  
  143.         /* Apply the transformation to the remaining columns */
  144.  
  145.         if (i + 1 < N)
  146.           {
  147.         gsl_matrix_view m = gsl_matrix_submatrix (A, i, i + 1, M - i, N - (i+1));
  148.  
  149.         gsl_linalg_householder_hm (tau_i, &c.vector, &m.matrix);
  150.           }
  151.       }
  152.  
  153.       /* Update the norms of the remaining columns too */
  154.  
  155.           if (i + 1 < M) 
  156.             {
  157.               for (j = i + 1; j < N; j++)
  158.                 {
  159.                   double y = 0;
  160.                   double x = gsl_vector_get (norm, j);
  161.  
  162.           if (x > 0.0)
  163.             {
  164.               double temp= gsl_matrix_get (A, i, j) / x;
  165.                   
  166.               if (fabs (temp) >= 1)
  167.             y = 0.0;
  168.               else
  169.             y = y * sqrt (1 - temp * temp);
  170.               
  171.               /* recompute norm to prevent loss of accuracy */
  172.  
  173.               if (fabs (y / x) < sqrt (20.0) * GSL_SQRT_DBL_EPSILON)
  174.             {
  175.               gsl_vector_view c_full = gsl_matrix_column (A, j);
  176.               gsl_vector_view c = 
  177.                 gsl_vector_subvector(&c_full.vector,
  178.                          i+1, M - (i+1));
  179.               y = gsl_blas_dnrm2 (&c.vector);
  180.             }
  181.                   
  182.               gsl_vector_set (norm, j, y);
  183.             }
  184.                 }
  185.             }
  186.         }
  187.  
  188.       return GSL_SUCCESS;
  189.     }
  190. }
  191.  
  192. int
  193. gsl_linalg_QRPT_decomp2 (const gsl_matrix * A, gsl_matrix * q, gsl_matrix * r, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
  194. {
  195.   const size_t M = A->size1;
  196.   const size_t N = A->size2;
  197.  
  198.   if (q->size1 != M || q->size2 !=M) 
  199.     {
  200.       GSL_ERROR ("q must be M x M", GSL_EBADLEN);
  201.     }
  202.   else if (r->size1 != M || r->size2 !=N)
  203.     {
  204.       GSL_ERROR ("r must be M x N", GSL_EBADLEN);
  205.     }
  206.   else if (tau->size != GSL_MIN (M, N))
  207.     {
  208.       GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
  209.     }
  210.   else if (p->size != N)
  211.     {
  212.       GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
  213.     }
  214.   else if (norm->size != N)
  215.     {
  216.       GSL_ERROR ("norm size must be N", GSL_EBADLEN);
  217.     }
  218.  
  219.   gsl_matrix_memcpy (r, A);
  220.  
  221.   gsl_linalg_QRPT_decomp (r, tau, p, signum, norm);
  222.  
  223.   /* FIXME:  aliased arguments depends on behavior of unpack routine! */
  224.  
  225.   gsl_linalg_QR_unpack (r, tau, q, r);
  226.  
  227.   return GSL_SUCCESS;
  228. }
  229.  
  230.  
  231. /* Solves the system A x = b using the Q R P^T factorisation,
  232.  
  233.    R z = Q^T b
  234.  
  235.    x = P z;
  236.  
  237.    to obtain x. Based on SLATEC code. */
  238.  
  239. int
  240. gsl_linalg_QRPT_solve (const gsl_matrix * QR,
  241.                const gsl_vector * tau,
  242.                const gsl_permutation * p,
  243.                const gsl_vector * b,
  244.                        gsl_vector * x)
  245. {
  246.   if (QR->size1 != QR->size2)
  247.     {
  248.       GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
  249.     }
  250.   else if (QR->size1 != p->size)
  251.     {
  252.       GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
  253.     }
  254.   else if (QR->size1 != b->size)
  255.     {
  256.       GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
  257.     }
  258.   else if (QR->size2 != x->size)
  259.     {
  260.       GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
  261.     }
  262.   else
  263.     {
  264.       gsl_vector_memcpy (x, b);
  265.  
  266.       gsl_linalg_QRPT_svx (QR, tau, p, x);
  267.       
  268.       return GSL_SUCCESS;
  269.     }
  270. }
  271.  
  272. int
  273. gsl_linalg_QRPT_svx (const gsl_matrix * QR,
  274.                      const gsl_vector * tau,
  275.                      const gsl_permutation * p,
  276.                      gsl_vector * x)
  277. {
  278.   if (QR->size1 != QR->size2)
  279.     {
  280.       GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
  281.     }
  282.   else if (QR->size1 != p->size)
  283.     {
  284.       GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
  285.     }
  286.   else if (QR->size2 != x->size)
  287.     {
  288.       GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
  289.     }
  290.   else
  291.     {
  292.       /* compute sol = Q^T b */
  293.  
  294.       gsl_linalg_QR_QTvec (QR, tau, x);
  295.  
  296.       /* Solve R x = sol, storing x inplace in sol */
  297.  
  298.       gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);
  299.  
  300.       gsl_permute_vector_inverse (p, x);
  301.  
  302.       return GSL_SUCCESS;
  303.     }
  304. }
  305.  
  306.  
  307. int
  308. gsl_linalg_QRPT_QRsolve (const gsl_matrix * Q, const gsl_matrix * R,
  309.              const gsl_permutation * p,
  310.              const gsl_vector * b,
  311.              gsl_vector * x)
  312. {
  313.   if (Q->size1 != Q->size2 || R->size1 != R->size2)
  314.     {
  315.       return GSL_ENOTSQR;
  316.     }
  317.   else if (Q->size1 != p->size || Q->size1 != R->size1
  318.        || Q->size1 != b->size)
  319.     {
  320.       return GSL_EBADLEN;
  321.     }
  322.   else
  323.     {
  324.       /* compute b' = Q^T b */
  325.  
  326.       gsl_blas_dgemv (CblasNoTrans, 1.0, Q, b, 0.0, x);
  327.  
  328.       /* Solve R x = b', storing x inplace */
  329.  
  330.       gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, R, x);
  331.  
  332.       /* Apply permutation to solution in place */
  333.  
  334.       gsl_permute_vector_inverse (p, x);
  335.  
  336.       return GSL_SUCCESS;
  337.     }
  338. }
  339.  
  340. int
  341. gsl_linalg_QRPT_Rsolve (const gsl_matrix * QR,
  342.             const gsl_permutation * p,
  343.                         const gsl_vector * b,
  344.             gsl_vector * x)
  345. {
  346.   if (QR->size1 != QR->size2)
  347.     {
  348.       GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
  349.     }
  350.   else if (QR->size1 != b->size)
  351.     {
  352.       GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
  353.     }
  354.   else if (QR->size2 != x->size)
  355.     {
  356.       GSL_ERROR ("matrix size must match x size", GSL_EBADLEN);
  357.     }
  358.   else if (p->size != x->size)
  359.     {
  360.       GSL_ERROR ("permutation size must match x size", GSL_EBADLEN);
  361.     }
  362.   else
  363.     {
  364.       /* Copy x <- b */
  365.  
  366.       gsl_vector_memcpy (x, b);
  367.  
  368.       /* Solve R x = b, storing x inplace */
  369.  
  370.       gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);
  371.  
  372.       gsl_permute_vector_inverse (p, x);
  373.  
  374.       return GSL_SUCCESS;
  375.     }
  376. }
  377.  
  378.  
  379. int
  380. gsl_linalg_QRPT_Rsvx (const gsl_matrix * QR,
  381.                       const gsl_permutation * p,
  382.                       gsl_vector * x)
  383. {
  384.   if (QR->size1 != QR->size2)
  385.     {
  386.       GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
  387.     }
  388.   else if (QR->size2 != x->size)
  389.     {
  390.       GSL_ERROR ("matrix size must match x size", GSL_EBADLEN);
  391.     }
  392.   else if (p->size != x->size)
  393.     {
  394.       GSL_ERROR ("permutation size must match x size", GSL_EBADLEN);
  395.     }
  396.   else
  397.     {
  398.       /* Solve R x = b, storing x inplace */
  399.  
  400.       gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);
  401.  
  402.       gsl_permute_vector_inverse (p, x);
  403.  
  404.       return GSL_SUCCESS;
  405.     }
  406. }
  407.  
  408.  
  409.  
  410. /* Update a Q R P^T factorisation for A P= Q R ,  A' = A + u v^T,
  411.  
  412.    Q' R' P^-1 = QR P^-1 + u v^T
  413.    = Q (R + Q^T u v^T P ) P^-1
  414.    = Q (R + w v^T P) P^-1
  415.  
  416.    where w = Q^T u.
  417.  
  418.    Algorithm from Golub and Van Loan, "Matrix Computations", Section
  419.    12.5 (Updating Matrix Factorizations, Rank-One Changes)  */
  420.  
  421. int
  422. gsl_linalg_QRPT_update (gsl_matrix * Q, gsl_matrix * R,
  423.             const gsl_permutation * p,
  424.             gsl_vector * w, const gsl_vector * v)
  425. {
  426.   if (Q->size1 != Q->size2 || R->size1 != R->size2)
  427.     {
  428.       return GSL_ENOTSQR;
  429.     }
  430.   else if (R->size1 != Q->size2 || v->size != Q->size2 || w->size != Q->size2)
  431.     {
  432.       return GSL_EBADLEN;
  433.     }
  434.   else
  435.     {
  436.       size_t j, k;
  437.       const size_t M = Q->size1;
  438.       const size_t N = Q->size2;
  439.       double w0;
  440.  
  441.       /* Apply Given's rotations to reduce w to (|w|, 0, 0, ... , 0) 
  442.  
  443.          J_1^T .... J_(n-1)^T w = +/- |w| e_1
  444.  
  445.          simultaneously applied to R,  H = J_1^T ... J^T_(n-1) R
  446.          so that H is upper Hessenberg.  (12.5.2) */
  447.  
  448.       for (k = N - 1; k > 0; k--)
  449.     {
  450.       double c, s;
  451.       double wk = gsl_vector_get (w, k);
  452.       double wkm1 = gsl_vector_get (w, k - 1);
  453.  
  454.       create_givens (wkm1, wk, &c, &s);
  455.       apply_givens_vec (w, k - 1, k, c, s);
  456.       apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
  457.     }
  458.  
  459.       w0 = gsl_vector_get (w, 0);
  460.  
  461.       /* Add in w v^T  (Equation 12.5.3) */
  462.  
  463.       for (j = 0; j < N; j++)
  464.     {
  465.       double r0j = gsl_matrix_get (R, 0, j);
  466.       size_t p_j = gsl_permutation_get (p, j);
  467.       double vj = gsl_vector_get (v, p_j);
  468.       gsl_matrix_set (R, 0, j, r0j + w0 * vj);
  469.     }
  470.  
  471.       /* Apply Givens transformations R' = G_(n-1)^T ... G_1^T H  
  472.          Equation 12.5.4 */
  473.  
  474.       for (k = 1; k < N; k++)
  475.     {
  476.       double c, s;
  477.       double diag = gsl_matrix_get (R, k - 1, k - 1);
  478.       double offdiag = gsl_matrix_get (R, k, k - 1);
  479.  
  480.       create_givens (diag, offdiag, &c, &s);
  481.       apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
  482.     }
  483.  
  484.       return GSL_SUCCESS;
  485.     }
  486. }
  487.